# -*-coding:utf-8-*-
import matplotlib
import matplotlib.pyplot as plt

matplotlib.use("Qt5Agg")  # 声明使用QT5
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure
from PyQt5 import QtWidgets, QtGui, QtCore
from window import Ui_MainWindow
from PyQt5.QtCore import QFile, QFileInfo, QIODevice, QTextStream
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure
import numpy as np
import cv2 as cv
from axis_transform import get_xy


class MyFigure(FigureCanvas):
    def __init__(self, parent=None, weight=5, height=4, dpi=100):
        fig = Figure(figsize=(weight, height), dpi=dpi)
        FigureCanvas.__init__(self, fig)  # 初始化父类
        self.setParent(parent)
        self.axes = fig.add_subplot(111)  # 调用figure下面的add_subplot方法，类似于matplotlib.pyplot下面的subplot方法

        self.mean = np.array([2, 1])
        self.conv = np.array([[0.5, 0.0], [0.0, 0.5]])
        self.last_measurement = self.current_measurement = np.array((get_xy(0)[0], get_xy(0)[1]), np.float32)
        self.last_prediction = self.current_prediction = np.array((get_xy(0)[0], get_xy(0)[1]), np.float32)
        self.kalman = cv.KalmanFilter(2, 2)
        self.kalman.measurementMatrix = np.array([[1, 0], [0, 1]], np.float32)
        self.kalman.transitionMatrix = np.array([[1, 0], [0, 1]], np.float32)
        self.kalman.processNoiseCov = np.array([[0.5, 0.0], [0.0, 0.5]], np.float32) * 0.003

    def predict(self, x, y):
        # global current_measurement, last_measurement, current_prediction, last_prediction
        self.last_prediction = self.current_prediction  # 把当前预测存储为上一次预测
        self.last_measurement = self.current_measurement  # 把当前测量存储为上一次测量
        self.current_measurement = np.array([[np.float32(x)], [np.float32(y)]])  # 当前测量
        self.kalman.correct(self.current_measurement)  # 用当前测量来校正卡尔曼滤波器
        self.current_prediction = self.kalman.predict()  # 计算卡尔曼预测值，作为当前预测

    def plot_Normal(self):
        global cpx, cpy
        for i in range(28):
            x, y = get_xy(i+1)
            # x, y = np.random.multivariate_normal(mean=self.mean, cov=self.conv, size=1).T
            self.predict(x, y)
            cpx, cpy = self.current_prediction[0], self.current_prediction[1]
            print(cpx, cpy)
            self.axes.plot(x, y, 'xr')  # x为×，r为红色

        self.axes.plot(cpx, cpy, 'go')

        # plt.show()
        print("========")
        print(type(cpx))
        return cpx, cpy

    def plot_position(self):
        global cpx, cpy

        for i in range(1000):
            x, y = np.random.multivariate_normal(mean=self.mean, cov=self.conv, size=1).T
            self.predict(x, y)
            cpx, cpy = self.current_prediction[0], self.current_prediction[1]
            print(cpx, cpy)
            self.axes.plot(x, y, 'xr')  # x为×，r为红色

        self.axes.plot(cpx, cpy, 'go')

        # plt.show()
        print("========")
        print(type(cpx))
        return cpx, cpy


class MainWindow(QtWidgets.QMainWindow, Ui_MainWindow):
    def __init__(self, parent=None):
        super(MainWindow, self).__init__(parent)
        self.setupUi(self)
        import random
        import numpy as np

        # ===通过graphicview来显示图形
        self.graphicview = Ui_MainWindow.graphicsView  # 第一步，创建一个QGraphicsView
        self.graphicview.setObjectName("graphicview")
        drOld = MyFigure()
        # 实例化一个FigureCanvas
        drOld.plotSin()  # 画图
        graphicscene = QtWidgets.QGraphicsScene()  # 第三步，创建一个QGraphicsScene，因为加载的图形（FigureCanvas）不能直接放到graphicview控件中，必须先放到graphicScene，然后再把graphicscene放到graphicview中
        graphicscene.addWidget(drOld)  # 第四步，把图形放到QGraphicsScene中，注意：图形是作为一个QWidget放到QGraphicsScene中的
        self.graphicview.setScene(graphicscene)  # 第五步，把QGraphicsScene放入QGraphicsView
        self.graphicview.show()  # 最后，调用show方法呈现图形！Voila!!


if __name__ == "__main__":
    import sys

    app = QtWidgets.QApplication(sys.argv)
    mainWindow = MainWindow()
    mainWindow.show()
    sys.exit(app.exec_())
